using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.Bullet.Dynamics
{
	public enum SolverMode
	{
		RandomizeOrder = 1,
		FrictionSeperate = 2,
		UseWarmstarting = 4,
	}

	public class SequentialImpulseConstraintSolver : IConstraintSolver
	{
		private static int _totalContactPoints = 0;

		private SolverMode _solverMode;
		private int _totalCpd = 0;
		private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
		private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];

		private const int _sequentialImpulseMaxSolverPoints = 16384;
		private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints];
		private static ulong _seed2 = 0;

		public SequentialImpulseConstraintSolver()
		{
			_solverMode = SolverMode.UseWarmstarting;
			PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback;

			//initialize default friction/contact funcs
			int i, j;
			for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++)
				for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++)
				{

					_contactDispatch[i, j] = ContactConstrain.ResolveSingleCollision;
					_frictionDispatch[i, j] = ContactConstrain.ResolveSingleFriction;
				}
		}

		public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } }
		public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } }
		protected static OrderIndex[] Order { get { return _order; } set { _order = value; } }

		///<summary>
		/// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
		/// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
		///</summary>
		public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB)
		{
			_contactDispatch[typeA, typeB] = func;
		}

		/// <summary>
		/// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
		/// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
		///</summary>
		public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB)
		{
			_frictionDispatch[typeA, typeB] = func;
		}

		protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
		{
			float maxImpulse = 0;

			Vector3 color = new Vector3(0, 1, 0);
			if (cp.Distance <= 0)
			{
				if (iter == 0)
					if(debugDraw != null)
						debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);

				ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
				float impulse = cpd.ContactSolverFunc(
					bodyA, bodyB,
					cp,
					info);

				if (maxImpulse < impulse)
					maxImpulse = impulse;
			}
			return maxImpulse;
		}

		protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
		{
			return Solve(bodyA, bodyB, cp, info, iter, null);
		}

		protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
		{
			Vector3 color = new Vector3(0, 1, 0);

			if (cp.Distance <= 0)
			{

				ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
				cpd.FrictionSolverFunc(
					bodyA, bodyB,
					cp,
					info);
			}
			return 0;
		}

		protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
		{
			RigidBody body0 = manifold.BodyA as RigidBody;
			RigidBody body1 = manifold.BodyB as RigidBody;


			//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
			{
				manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);

				int numpoints = manifold.ContactsCount;

				_totalContactPoints += numpoints;

				Vector3 color = new Vector3(0, 1, 0);
				for (int i = 0; i < numpoints; i++)
				{
					ManifoldPoint cp = manifold.GetContactPoint(i);
					if (cp.Distance <= 0)
					{
						Vector3 pos1 = cp.PositionWorldOnA;
						Vector3 pos2 = cp.PositionWorldOnB;

						Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
						Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;


						//this jacobian entry is re-used for all iterations
						JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
							MatrixOperations.Transpose(body1.CenterOfMassTransform),
							rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
							body1.InvInertiaDiagLocal, body1.InverseMass);

						float jacDiagAB = jac.Diagonal;

						ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
						if (cpd != null)
						{
							//might be invalid
							cpd.PersistentLifeTime++;
							if (cpd.PersistentLifeTime != cp.LifeTime)
							{
								//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
								cpd = new ConstraintPersistentData();
								cpd.PersistentLifeTime = cp.LifeTime;

							}
						}
						else
						{

							cpd = new ConstraintPersistentData();
							_totalCpd++;
							//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
							cp.UserPersistentData = cpd;
							cpd.PersistentLifeTime = cp.LifeTime;
							//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());

						}
						if (cpd == null)
							throw new BulletException();

						cpd.JacDiagABInv = 1f / jacDiagAB;

						//Dependent on Rigidbody A and B types, fetch the contact/friction response func
						//perhaps do a similar thing for friction/restutution combiner funcs...

						cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
						cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];

						Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
						Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
						Vector3 vel = vel1 - vel2;
						float rel_vel;
						rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);

						float combinedRestitution = cp.CombinedRestitution;

						cpd.Penetration = cp.Distance;
						cpd.Friction = cp.CombinedFriction;
						cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
						if (cpd.Restitution < 0f)
						{
							cpd.Restitution = 0.0f;

						};

						//restitution and penetration work in same direction so
						//rel_vel 

						float penVel = -cpd.Penetration / info.TimeStep;

						if (cpd.Restitution > penVel)
						{
							cpd.Penetration = 0;
						}


						float relaxation = info.Damping;
						if ((_solverMode & SolverMode.UseWarmstarting) != 0)
						{
							cpd.AppliedImpulse *= relaxation;
						}
						else
						{
							cpd.AppliedImpulse = 0f;
						}

						//for friction
						cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;

						//re-calculate friction direction every frame, todo: check if this is really needed
						Vector3 fwta = cpd.FrictionWorldTangentialA;
						Vector3 fwtb = cpd.FrictionWorldTangentialB;
						MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
						cpd.FrictionWorldTangentialA = fwta;
						cpd.FrictionWorldTangentialB = fwtb;

						cpd.AccumulatedTangentImpulseA = 0;
						cpd.AccumulatedTangentImpulseB = 0;
						float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
						float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
						float denom = relaxation / (denom0 + denom1);
						cpd.JacDiagABInvTangentA = denom;


						denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
						denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
						denom = relaxation / (denom0 + denom1);
						cpd.JacDiagABInvTangentB = denom;

						Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;

						{
							Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
							cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
							Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
							cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
						}
						{
							Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
							cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
						}
						{
							Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
							cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
						}
						{
							Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
							cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
						}
						{
							Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
							cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
						}


						//apply previous frames impulse on both bodies
						body0.ApplyImpulse(totalImpulse, rel_pos1);
						body1.ApplyImpulse(-totalImpulse, rel_pos2);
					}
				}
			}
		}

		private bool MyContactDestroyedCallback(object userPersistentData)
		{
			if (userPersistentData == null)
				throw new BulletException();
			ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData;
			_totalCpd--;
			return true;
		}

		private float RestitutionCurve(float relVel, float restitution)
		{
			float rest = restitution * -relVel;
			return rest;
		}

		public virtual float SolveGroup(PersistentManifold[] manifold, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
		{
			ContactSolverInfo info = infoGlobal;

			int numiter = infoGlobal.IterationsCount;

			{
				int j;
				for (j = 0; j < manifold.Length; j++)
				{
					PersistentManifold manifold2 = manifold[j];
					PrepareConstraints(manifold2, info);
					for (int p = 0; p < manifold[j].ContactsCount; p++)
					{
						//interleaving here gives better results
						Solve((RigidBody)manifold2.BodyA, (RigidBody)manifold2.BodyB, manifold[j].GetContactPoint(p), info, 0, debugDrawer);
					}
				}
			}

			//should traverse the contacts random order...
			int iteration;
			
			for (iteration = 0; iteration < numiter - 1; iteration++)
			{
				int j;

				for (j = 0; j < manifold.Length; j++)
				{
					PersistentManifold manifold2 = manifold[j];
					for (int p = 0; p < manifold2.ContactsCount; p++)
					{
						Solve((RigidBody)manifold2.BodyA, (RigidBody)manifold2.BodyB, manifold2.GetContactPoint(p), info, iteration, debugDrawer);
					}
				}
			}

			for (iteration = 0; iteration < numiter - 1; iteration++)
			{
				int j;
				for (j = 0; j < manifold.Length; j++)
				{
					PersistentManifold manifold2 = manifold[j];
					for (int p = 0; p < manifold2.ContactsCount; p++)
					{
						SolveFriction((RigidBody)manifold2.BodyA, (RigidBody)manifold2.BodyB, manifold2.GetContactPoint(p), info, iteration, debugDrawer);
					}
				}
			}
			return 0;
		}

		protected ulong Rand2()
		{
			_seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff;
			return _seed2;
		}

		protected int RandInt2(int n)
		{
			// seems good; xor-fold and modulus
			ulong un = (ulong)n;
			ulong r = Rand2();

			// note: probably more aggressive than it needs to be -- might be
			//       able to get away without one or two of the innermost branches.
			if (un <= 0x00010000UL)
			{
				r ^= (r >> 16);
				if (un <= 0x00000100UL)
				{
					r ^= (r >> 8);
					if (un <= 0x00000010UL)
					{
						r ^= (r >> 4);
						if (un <= 0x00000004UL)
						{
							r ^= (r >> 2);
							if (un <= 0x00000002UL)
							{
								r ^= (r >> 1);
							}
						}
					}
				}
			}
			return (int)(r % un);
		}

		protected int RandIntWrong(int n)
		{
			float a = (float)(n) / 4294967296.0f;
			//  printf("n = %d\n",n);
			//  printf("a = %f\n",a);
			int res = (int)((float)(Rand2()) * a);
			//	printf("res=%d\n",res);
			return res;
		}

		protected struct OrderIndex
		{
			private short _manifoldIndex;
			private short _pointIndex;

			public short ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } }
			public short PointIndex { get { return _pointIndex; } set { _pointIndex = value; } }
		}
	}

	public class SequentialImpulseConstraintSolver3 : SequentialImpulseConstraintSolver
	{
		public SequentialImpulseConstraintSolver3()
		{
			SolverMode = SolverMode.RandomizeOrder;
		}

		public override float SolveGroup(PersistentManifold[] manifold, ContactSolverInfo infoGlobal, IDebugDraw debugDraw)
		{
			ContactSolverInfo info = infoGlobal;

			int numiter = infoGlobal.IterationsCount;
			int totalPoints = 0;

			for (int j = 0; j < manifold.Length; j++)
			{
				PersistentManifold manifold2 = manifold[j];
				PrepareConstraints(manifold2, info);
				for (int p = 0; p < manifold[j].ContactsCount; p++)
				{
					Order[totalPoints].ManifoldIndex = (short)j;
					Order[totalPoints].PointIndex = (short)p;
					totalPoints++;
				}
			}

			//should traverse the contacts random order...
			int iteration;
			{
				for (iteration = 0; iteration < numiter - 1; iteration++)
				{
					int j;
					if ((SolverMode & SolverMode.RandomizeOrder) != 0)
					{
						if ((iteration & 7) == 0)
						{
							for (j = 0; j < totalPoints; ++j)
							{
								OrderIndex tmp = Order[j];
								int swapi = RandInt2(j + 1);
								Order[j] = Order[swapi];
								Order[swapi] = tmp;
							}
						}
					}

					for (j = 0; j < totalPoints; j++)
					{
						PersistentManifold manifold2 = manifold[Order[j].ManifoldIndex];
						Solve((RigidBody)manifold2.BodyA,
											(RigidBody)manifold2.BodyB, manifold2.GetContactPoint(Order[j].PointIndex), info, iteration, debugDraw);
					}

					for (j = 0; j < totalPoints; j++)
					{
						PersistentManifold manifold2 = manifold[Order[j].ManifoldIndex];
						SolveFriction((RigidBody)manifold2.BodyA,
							(RigidBody)manifold2.BodyB, manifold2.GetContactPoint(Order[j].PointIndex), info, iteration, debugDraw);
					}
				}
			}

			return 0;
		}
	}
}
